/*
 * Other.h
 *
 *  Created on: Jun 10, 2024
 *      Author: Liangmiyi
 */
#ifndef INC_Other_H_
#define INC_Other_H_
#include <MY_INC/PHY/PHY_Cordic.h>
#include <MY_INC/PHY/PHY_math.h>
#include <stdint.h>

// PLL 结构体
typedef struct
{
    float Input; // 比例增益
    // PI 控制器参数
    float Coeff_B0; // 比例增益
    float Coeff_B1; // 积分增益

    float Yn;   // 上一次输出
    float Xn_1; // 上一次输出

    // 相位和频率
    float W;     // 角频率
    float Theta; // 相位

    // 状态变量
    float Integral_Xn_1; // 上一次输入
    float Integral_Yn;   // 上一次输出
    float ts_half;       // 0.5 * ts，用于计算

    float Feedforward; // 前馈值

    float Sine;   // 正弦值
    float Cosine; // 余弦值
} PLL_Typedef;

/**
 * @brief PLL初始化函数
 * @param PLL PLL结构体指针
 * @param Kp 比例增益
 * @param Ki 积分增益
 * @param Feedforward 前馈值
 * @param ts 采样周期
 */
void PLL_Init(PLL_Typedef *PLL, float Kp, float Ki, float Feedforward, float ts);
void PLL_Reset(PLL_Typedef *PLL);
/**
 * @brief PLL计算函数
 * @param PLL PLL结构体指针
 * @param Actual 实际值
 */
static inline void PLL_Process_Inline(PLL_Typedef *PLL, float Actual)
{
    /*===================== PI控制器计算 =====================*/
    PLL->Input = Actual * 0.01f + 0.99f * PLL->Input; // 低通滤波器
    // PLL->Input = Actual; // 低通滤波器
    PLL->Yn += PLL->Coeff_B0 * PLL->Input + PLL->Coeff_B1 * PLL->Xn_1;
    PLL->Xn_1 = PLL->Input; // 更新上一次输入

    /*===================== 计算积分输出 =====================*/
    // 计算输出
    //    float Integral_Xn = Yn + PLL->Feedforward;
    float Integral_Xn = PLL->Yn + 314.1592653f;

    // % 计算积分输出
    PLL->Integral_Yn += PLL->ts_half * (Integral_Xn + PLL->Integral_Xn_1);
    if (PLL->Integral_Yn > _2PI)
        PLL->Integral_Yn -= _2PI;
    else if (PLL->Integral_Yn < 0.0f)
        PLL->Integral_Yn += _2PI;
    // 更新状态
    PLL->Theta = PLL->Integral_Yn;
    PLL->W = PLL->Integral_Xn_1 = Integral_Xn;
    // Cordic计算正弦和余弦值
    Cordic_SinCos_Inline(PLL->Theta, &PLL->Sine, &PLL->Cosine);
}

#endif /* INC_Other_H_ */
